#include "Bot.h"
#include "level.h"
#include "A-Star.h"
#include "GraphVisualisation.h"
#include "line.h"
#include <algorithm>
#include <Clarity/Math/Random.h>
#include <Clarity/Math/Direction.h>
#include <Clarity/Math/LineSegment2.h>
#include <Clarity/Math/Circle.h>
#include <Clarity/Math/Math.h>

extern "C"
{
   BOT_API void CreateAgent(SharedPtr<IAgent> &agent, AgentInitData agentInitData)
   {
      agent = SharedPtr<IAgent>(new Bot(agentInitData));
   }
}

namespace
{
   const float RADIUS = 16.0f;
   const float CORNER_DISPLACEMENT = RADIUS + (RADIUS * 0.5f);

   const float APPROACH_NODE_SLOW_DOWN_DIST = 16.0f;
   const float ARRIVAL_DISTANCE_TOLERANCE = 4.0f;
}

Bot::Bot(const AgentInitData &agentInitData)
   : m_agentInitData(agentInitData), m_StartUp(true), m_State(s_Check)
{
	m_AStar = new AStar();
	m_NextNode = NULL;
	//Create Level
	CreateLevel();
	//Create Nodes;
	CreateNodes();

	ConnectNodes();

	/*
	*	DEBUGING
	*/
	//m_Nodes = new GraphVisualisation(m_AStar,m_agentInitData.sceneManager);
	m_Lines = new Lines(m_agentInitData.sceneManager);
}

Bot::~Bot()
{
	//delete m_Lines;
	delete m_AStar; 
	//delete m_Nodes;
	delete m_Level;
	m_NextNode = NULL;
}

void Bot::Step(const AgentStateIn &agentStateIn, AgentStateOut &agentStateOut)
{
	if(m_StartUp)
	{
		m_NextNode = ClosestNode(agentStateIn.position);
		agentStateOut.power = 1.0f;
		m_StartUp = false;
	}

	if(m_NextNode != NULL)
	{

		std::vector<SensorData>::const_iterator it;
		std::vector<SensorData>::const_iterator itEnd = agentStateIn.sensorData.end();
		float Distance = 0.0f;
		Vector2 Direction;
		static float angle = 1.0f;
		static int count = 0;

		for (it = agentStateIn.sensorData.begin(); it != itEnd; ++it)
		{
			const SensorData &sensorData(*it);

			if (sensorData.type == "Agent")
			{
				agentStateOut.lookDir = sensorData.position - agentStateIn.position;
				m_Target = sensorData.position;
				if(m_State == s_Seek)
					m_State = s_Path;
			}
		}
		switch(m_State)
		{
		case s_Seek:
			angle += 0.12f;
			agentStateOut.lookDir.x += Cos(angle);
			agentStateOut.lookDir.y -= Sin(angle);
			agentStateOut.action = ACTION_SCAN;
			break;
		case s_Path:
			//m_Lines->clear();
			m_AStar->clearnodes();
			m_NextNode = (ClosestNode(agentStateIn.position));
			m_AStar->GetPath(m_NextNode,(ClosestNode(m_Target)));
			//drawPath();

			m_State = s_Destroy;
			break;
		case s_Check:
			//drawPath();
			m_State = s_Seek;
			break;
		case s_Destroy:
			if(m_NextNode->m_Next == NULL)
				m_State = s_Seek;
			//Check LoS
			if (m_Level->IsPathBetween(agentStateIn.position,m_Target,RADIUS))
			{
				agentStateOut.action = ACTION_FIRE;
				++count;
				if(count == 2)
				{
					m_State = s_Seek;
					count = 0;
				}
			}
			break;
		};
		if(m_Level->IsPathBetween(agentStateIn.position,m_Target,RADIUS))
		{
			Direction = Vector2(m_Target - agentStateIn.position);
			Distance = Direction.Normalise();
		} else 
		{
			Direction = Vector2(m_NextNode->m_Pos - agentStateIn.position);
			Distance = Direction.Normalise();
		}
		


		if(Distance <= ARRIVAL_DISTANCE_TOLERANCE)
		{
			if(m_NextNode->m_Next != NULL)
			{
				m_NextNode = m_NextNode->m_Next;
				Direction = Vector2(m_NextNode->m_Pos - agentStateIn.position);
				Distance = Direction.Normalise();
			}
			
		}
		agentStateOut.moveDir = Direction;

		if (Distance < APPROACH_NODE_SLOW_DOWN_DIST)
		{
			agentStateOut.power = Distance / APPROACH_NODE_SLOW_DOWN_DIST;
		}
		else
		{
			agentStateOut.power = 1.0f;
		}

	} else
	{
		agentStateOut.power = 1.0f;
	}

}

void Bot::drawPath()
{

	ANode* temp = NULL;
	int count = m_AStar->NodeCount();
	vector<ANode*>::iterator iter;
	vector<ANode*>::iterator iterEnd;

	for (int i = 0; i < count; ++i)
	{
		temp = m_AStar->GetNode(i);
		iter = temp->m_NodeList.begin();
		iterEnd = temp->m_NodeList.end();
		for (; iter != iterEnd; ++iter)
		{
			m_Lines->addLine(temp->m_Pos, (*iter)->m_Pos,255,0,0);
		}
		//m_Lines->clear();
	}

	//for( ;temp->m_Next != NULL; )
	//{
	//	m_Lines->addLine(temp->m_Pos, temp->m_Next->m_Pos,255,0,0);
	//	temp = temp->m_Next;
	//}
}


void Bot::CreateLevel()
{
	m_Level = new Level(m_agentInitData.arenaBounds,m_agentInitData);
	
	vector<AARect>::const_iterator iter;
	vector<AARect>::const_iterator iterEnd = m_agentInitData.walls.end();
	for (iter = m_agentInitData.walls.begin();
		iter != iterEnd;
		++iter)
	{
		m_Level->AddWall(*iter);
	}
}

void Bot::CreateNodes()
{
	//m_AStar = new AStar();

	//SelectGraphNodes
	vector<AARect>::const_iterator iter;
	vector<AARect>::const_iterator iterEnd = m_agentInitData.walls.end();
	for (iter = m_agentInitData.walls.begin();
		iter != iterEnd;
		++iter)
	{
		//SelectRectCornersasnodes
		const Vector2 *corners = (*iter).GetAllCorners();
		int currDir = SOUTH_WEST;
		for (int i = 0; i < 4; ++i)
		{
			Vector2 nodePos = corners[i] + DirectionToVector(currDir) * CORNER_DISPLACEMENT;

			if(!m_Level->Intersects(nodePos))
			{
				m_AStar->CreateNode(nodePos);
			}

			currDir = NextDiagonalDirection(currDir);
		}
	}
}

void Bot::ConnectNodes()
{
	int Count = m_AStar->NodeCount();

	for (int i = 0; i < Count; ++i)
	{
		for (int j = 0; j < Count; ++j)
		{
			if (i != j)
			{
				ANode* One = m_AStar->GetNode(i);
				ANode* Two = m_AStar->GetNode(j);
				bool Node = IsNodeBetween(One, Two);
				bool Path = m_Level->IsPathBetween(One->m_Pos, Two->m_Pos, RADIUS);
				if (!Node && Path)
				{
					m_AStar->ConnectNodes(One, Two);
				}
			}
		}
	}
}

bool Bot::IsNodeBetween(ANode* s_One, ANode* s_Two) const
{
	int Count = m_AStar->NodeCount();
	LineSegment2 between(s_One->m_Pos,s_Two->m_Pos);

	for (int i = 0; i < Count; i++)
	{
		ANode *currNode = m_AStar->GetNode(i);

		if (currNode != s_One && currNode != s_Two)
		{
			if (Intersects(between, Circle(currNode->m_Pos, RADIUS)))
			{
				return true;
			}
		}
	}

	return false;
}

ANode* Bot::ClosestNode(const Clarity::Vector2 &s_Pos)
{
	int count = m_AStar->NodeCount();
	vector<pair<float, ANode*>> list;

	for (int i = 0; i < count; ++i)
	{
		ANode* node = m_AStar->GetNode(i);
		float distance;
		
		if(m_Level->IsPathBetween(node->m_Pos, s_Pos,RADIUS))
		{
			distance = abs(( (s_Pos.x - node->m_Pos.x) + (s_Pos.y - node->m_Pos.y) ));
			list.push_back(make_pair(distance,node));
		}
	}

	sort(list.begin(),list.end());

	if (!list.empty())
	{
		return list.front().second;
	} else 
	{
		return NULL;
	}
}